
extern "C" {
#include "conjgrad_mt_state.h"
#include "lrwwsimplex_state.h"
#include "quasinewton_state.h"
}
#include "MinimizerStateCollector.h"
#include "OctaveUtils.h"

#include <gsl/gsl_blas.h>
#include <octave/Cell.h>
#include <octave/oct-map.h>
#include <stdexcept>

MinimizerStateCollector::MinimizerStateCollector(int n)
{
  g_ = gsl_vector_alloc(n);
  H_ = gsl_matrix_alloc(n, n);
}

MinimizerStateCollector::~MinimizerStateCollector()
{
  gsl_vector_free(g_);
  gsl_matrix_free(H_);
}

void MinimizerStateCollector::collectStateData(const GSLMinimizerWrapper &s,
                                               const GSLFunctionWrapper_fd2f &f,
                                               const StoppingCriterion &stopCrit,
                                               Octave_map &result)
{
  const int N = f.n();
  
  double eps;
  ColumnVector x_oct(N);
  ColumnVector g_oct(N);
  Matrix H_oct(N, N);
  
  x_oct = OctaveUtils::GSLVectorToOctaveVector(s.x());
  result.assign("x", x_oct);
  
  result.assign("f", s.f());
  
  if(s.getType() == GSLMinimizerWrapper::FDF || 
     s.getType() == GSLMinimizerWrapper::FD2F)
    g_oct = OctaveUtils::GSLVectorToOctaveVector(s.gradient());
  else
  {
    f.df(s.x(), g_);
    g_oct = OctaveUtils::GSLVectorToOctaveVector(g_);
  }
  
  result.assign("g", g_oct);
  
  if(s.getType() == GSLMinimizerWrapper::FD2F)
  {
    H_oct = OctaveUtils::GSLMatrixToOctaveMatrix(s.Hessian());
    result.assign("H", H_oct);
  }
  else if(s.getType() == GSLMinimizerWrapper::FDF)
  {
    f.d2f(s.x(), H_);
    H_oct = OctaveUtils::GSLMatrixToOctaveMatrix(H_);
    result.assign("H", H_oct);
  }
  
  result.assign("stopcrit_value", stopCrit.getThresholdValue(s));
  
  collectAlgorithmSpecificData_(s, result);
}

BFGSStateCollector::BFGSStateCollector(const string algorithmName, int n) : 
  MinimizerStateCollector(n), ALGORITHM_NAME_(algorithmName) { }

void BFGSStateCollector::collectAlgorithmSpecificData_(const GSLMinimizerWrapper &s,
                                                       Octave_map &resultMap) const
{
  if(s.getName() != "bfgs_hess_f" && 
     s.getName() != "bfgs_hess_mt" && 
     s.getName() != "bfgs_f" && 
     s.getName() != "bfgs_mt")
    throw invalid_argument("the given algorithm is not a GSL++ BFGS algorithm");
  
  bfgs_state_t *state = (bfgs_state_t *)s.getState();
  
  resultMap.assign("alpha", state->alpha);
  if(ALGORITHM_NAME_.find("hess") != string::npos)
    resultMap.assign("B", OctaveUtils::GSLMatrixToOctaveMatrix(state->H));
  else
    resultMap.assign("S", OctaveUtils::GSLMatrixToOctaveMatrix(state->H));
}

ConjGradStateCollector::ConjGradStateCollector(int n) : 
  MinimizerStateCollector(n) { }

void ConjGradStateCollector::collectAlgorithmSpecificData_(const GSLMinimizerWrapper &s,
                                                           Octave_map &resultMap) const
{
  if(s.getName() != "conjgrad_fr_mt" && 
     s.getName() != "conjgrad_pr_mt")
    throw invalid_argument("the given algorithm is not a GSL++ conjugate gradient algorithm");
  
  conjgrad_mt_state_t *state = (conjgrad_mt_state_t *)s.getState();
  resultMap.assign("d", OctaveUtils::GSLVectorToOctaveVector(state->d));
}

SimplexStateCollector::SimplexStateCollector(int n) : 
  MinimizerStateCollector(n) { }

void SimplexStateCollector::collectAlgorithmSpecificData_(const GSLMinimizerWrapper &s,
                                                          Octave_map &resultMap) const
{
  if(s.getName() != "lrwwsimplex")
    throw invalid_argument("the given algorithm is not a GSL++ simplex algorithm");
  
  const int N = s.getN();
  int i;
  lrwwsimplex_state_t *state = (lrwwsimplex_state_t *)s.getState();
  
  gsl_matrix *E_gsl = gsl_matrix_alloc(N, N);
  gsl_vector *tmpv = gsl_vector_alloc(N);
  Matrix E;
  Cell v(N + 1, 1);
  
  for(i = 0; i < N + 1; i++)
    v(i, 0) = OctaveUtils::GSLVectorToOctaveVector(state->S->v[i]);
  
  for(i = 0; i < N; i++)
  {
    gsl_blas_dcopy(state->S->v[0], tmpv);
    gsl_blas_dscal(-1.0, tmpv);
    gsl_blas_daxpy(1.0, state->S->v[i + 1], tmpv);
    gsl_matrix_set_col(E_gsl, i, tmpv);
  }
  
  resultMap.assign("simplex", octave_value(v));
  E = OctaveUtils::GSLMatrixToOctaveMatrix(E_gsl);
  resultMap.assign("E", E);
  resultMap.assign("V", state->S->V);
  
  gsl_vector_free(tmpv);
  gsl_matrix_free(E_gsl);
}

MinimizerStateCollector *MinimizerStateCollectorFactory::getInstance(const string &algorithmName, int n)
{
  if(algorithmName == "gsl_ext_bfgs_hess_mt" || 
     algorithmName == "gsl_ext_bfgs_mt" || 
     algorithmName == "gsl_ext_bfgs_hess_f" || 
     algorithmName == "gsl_ext_bfgs_f")
    return new BFGSStateCollector(algorithmName, n);
  else if(algorithmName == "gsl_ext_conjgrad_pr_mt" || 
          algorithmName == "gsl_ext_conjgrad_fr_mt")
    return new ConjGradStateCollector(n);
  else if(algorithmName == "gsl_ext_lrwwsimplex")
    return new SimplexStateCollector(n);
  else
    return new MinimizerStateCollector(n);
}
